#pragma once

#include "btBulletDynamicsCommon.h"
#include "GraphManager.h"
//#include "Triangle.h"
#include "World.h"

#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"
#include "ConvexHull.h"

#include <cmath>

/// Struktora zawierajaca niezbedne dane dla procesu dekompozycji.
struct DecompositionDesc
{
	DecompositionDesc() : mMinAABB(btVector3(0,0,0)), mMaxAABB(btVector3(0,0,0)), mStepSize(btVector3(1,1,1)) {}

	/// Lista obiektow sceny
	btAlignedObjectArray<ConvexHull*> mConvexHulls;

	/// Wskaznik na klase swiata.
	btCollisionWorld* mWorld;
	/// Skrajny, lewy, najblizszy wierzcholek AABB swiata.
	btVector3& mMinAABB;
	/// Skrajny, prawy, najdalszy wierzcholek AABB swiata.
	btVector3& mMaxAABB;
	/// Wektor opisujacy wielkosc kroku.
	btVector3& mStepSize;
};

/// Reprezentuje plaszczyzne.
class Plane : public btPlane
{
public:
	/// Operator porownania dwoch plaszczyzn.
	bool operator==(const Plane& plane)
	{
		return plane.normal == this->normal && plane.dist == this->dist;
	}
};

/// Opisuje krawedz bryly.
class Edge
{
public:
	/**Konstruktor
	* @param b Numer wierzcholka poczatkowego.
	* @param e Numer wierzchilka koncowego.
	*/
	Edge(int b, int e) : Begin(b), End(e), Counts(0)
	{			
	}

	/// Numer wierzcholka poczatkowego.
	int Begin;
	/// Numer wierzcholka koncowego.
	int End;

	/// Liczba wystapien.
	int Counts;

	/// Operator porownania.
	bool operator==(const Edge& p)
	{
		return (p.Begin == Begin && p.End == End) || (p.Begin == End && p.End == Begin);
	}
};

/// Opisuje sciane bryly.
class Face
{
public:
	/// Konstruktor.
	Face(int count = 1) : count(count)
	{
		CanBeUsed = true;
	}

	/// Oznacza sciane jako uzyta.
	void Use()
	{
		--count;
		CanBeUsed = count > 0;		
	}

	/// Operator porownania
	bool operator==(const Face& f)
	{
		if(f.Indices.size() == this->Indices.size())
		{
			for(int i = 0; i < f.Indices.size(); ++i)
			{
				if(this->Indices.findLinearSearch(f.Indices[i]) == this->Indices.size())
					return false;
			}
			return true;
		}

		return false;
	}

	/// Lista wierzcholkow tworzacych sciane.
	btAlignedObjectArray<int> Indices;
	/// Plaszczyzna opisujaca normalna sciany.
	btPlane Plane;
	/// Wspolrzedne wierzcholka srodka sciany.
	btVector3 Center;
	/// Flaga opisujaca, czy sciana moze byc jeszcze uzyta.
	bool CanBeUsed;
	/// Lista krawedzi sciany.
	btAlignedObjectArray<int> Edges;

	/// Liczba okreslajaca, ile razy sciana moze zostac uzyta.
	int count;
};

/// Przeprowadza dekompozycje swiata.
class Decomposition
{
public:

	/// Lista wyznaczonych obiektow.
	btAlignedObjectArray<ConvexHull*> mHulls;
	/// AABB swiata.
	ConvexHull* mWorldHull;
	/// Lista krawedzi.
	btAlignedObjectArray<Edge> mEdges;
	/// Lista plaszczyzn AABB.
	btAlignedObjectArray<Plane> mDefaultPlanes;

	/// Konstruktor.
	Decomposition(DecompositionDesc& desc) //: mTestCube(nullptr)
	{
		/*desc.mVertices.push_back(btVector3(desc.mMinAABB.getX(), desc.mMinAABB.getY(), desc.mMinAABB.getZ()));
		desc.mVertices.push_back(btVector3(desc.mMinAABB.getX(), desc.mMinAABB.getY(), desc.mMaxAABB.getZ()));
		desc.mVertices.push_back(btVector3(desc.mMaxAABB.getX(), desc.mMinAABB.getY(), desc.mMinAABB.getZ()));
		desc.mVertices.push_back(btVector3(desc.mMaxAABB.getX(), desc.mMinAABB.getY(), desc.mMaxAABB.getZ()));

		desc.mVertices.push_back(btVector3(desc.mMinAABB.getX(), desc.mMaxAABB.getY(), desc.mMinAABB.getZ()));
		desc.mVertices.push_back(btVector3(desc.mMinAABB.getX(), desc.mMaxAABB.getY(), desc.mMaxAABB.getZ()));
		desc.mVertices.push_back(btVector3(desc.mMaxAABB.getX(), desc.mMaxAABB.getY(), desc.mMinAABB.getZ()));
		desc.mVertices.push_back(btVector3(desc.mMaxAABB.getX(), desc.mMaxAABB.getY(), desc.mMaxAABB.getZ()));
		*/

		btAlignedObjectArray<btVector3> aabb;
		aabb.push_back(btVector3(desc.mMinAABB.getX(), desc.mMinAABB.getY(), desc.mMinAABB.getZ()));
		aabb.push_back(btVector3(desc.mMinAABB.getX(), desc.mMinAABB.getY(), desc.mMaxAABB.getZ()));
		aabb.push_back(btVector3(desc.mMaxAABB.getX(), desc.mMinAABB.getY(), desc.mMinAABB.getZ()));
		aabb.push_back(btVector3(desc.mMaxAABB.getX(), desc.mMinAABB.getY(), desc.mMaxAABB.getZ()));

		aabb.push_back(btVector3(desc.mMinAABB.getX(), desc.mMaxAABB.getY(), desc.mMinAABB.getZ()));
		aabb.push_back(btVector3(desc.mMinAABB.getX(), desc.mMaxAABB.getY(), desc.mMaxAABB.getZ()));
		aabb.push_back(btVector3(desc.mMaxAABB.getX(), desc.mMaxAABB.getY(), desc.mMinAABB.getZ()));
		aabb.push_back(btVector3(desc.mMaxAABB.getX(), desc.mMaxAABB.getY(), desc.mMaxAABB.getZ()));

		this->mWorldHull = new ConvexHull();
		this->mWorldHull->compute(&aabb[0].getX(), sizeof(btVector3), aabb.size(),0.0,0);

		this->mWorldHull->CorrectPrecision();
		this->mWorldHull->ComputePlanes();


		for(int i = 0; i < this->mWorldHull->vertices.size(); ++i)
		{
			mVertices.push_back(this->mWorldHull->vertices[i]);
		}

		//memcpy(&mGraph, &graph, sizeof(boost::multi_array<float, 2>));

		mMinAABB = desc.mMinAABB;
		mMaxAABB = desc.mMaxAABB;
		mWorld = desc.mWorld;
		mStepSize = desc.mStepSize;

		std::cout << "Hulls: " << desc.mConvexHulls.size() << std::endl;
		for(int i = 0; i < desc.mConvexHulls.size(); ++i)
		{
			mHulls.push_back(desc.mConvexHulls[i]);
			std::cout << "\t" << desc.mConvexHulls[i]->vertices.size() << std::endl;
			for(int j = 0; j < desc.mConvexHulls[i]->vertices.size(); ++j)
				mVertices.push_back(desc.mConvexHulls[i]->vertices[j]);
		}		
		int size = mVertices.size();

		std::cout << "Vertices:" << std::endl;
		for(int i = 0; i < size; ++i)
		{
			std::cout << "\t";
			//Triangle::Print(mVertices[i]);
			std::cout << std::endl;
		}

		boost::multi_array<float, 2> graph = GraphManager::CreateGraph(mVertices, mWorld);	


		mGraph.resize(boost::extents[size][size]);
		for(int i = 0; i < size; ++i)
		{
			for(int j = 0; j < size; ++j)
			{
				mGraph[i][j] = mGraph[j][i] = graph[i][j];
			}
		}


		// Tworzenie listy scian
		for(int i = 0; i < mWorldHull->faces.size(); ++i)
		{
			btAlignedObjectArray<btVector3> tmp;
			mWorldHull->GetVerticesFromFace(i, tmp);
			Face face;
			face.Center.setZero();
			face.Plane = mWorldHull->planes[i];
			for(int j = 0; j < tmp.size(); ++j)
			{
				face.Center += tmp[j];
				face.Indices.push_back(mVertices.findLinearSearch(tmp[j]));
			}
			face.Center /= tmp.size();
			tmp.clear();

			mWorldHull->GetEdgesFromFace(i, tmp);
			for(int j = 0; j < tmp.size(); j+=2)
			{
				Edge* edge = new Edge(mVertices.findLinearSearch(tmp[j]), mVertices.findLinearSearch(tmp[j+1]));
				int index = mEdges.findLinearSearch(*edge);
				if(index < mEdges.size())
				{
					delete edge;						
				}
				else
				{
					mEdges.push_back(*edge);
				}
				face.Edges.push_back(index);
			}

			mFaces.push_back(face);
		}

		for(int i = 0; i < mHulls.size(); ++i)
		{
			for(int j = 0; j < mHulls[i]->faces.size(); ++j)
			{
				btAlignedObjectArray<btVector3> tmp;
				mHulls[i]->GetVerticesFromFace(j,tmp);
				Face face;
				face.Center.setZero();
				face.Plane = mHulls[i]->planes[j];
				for(int k = 0; k < tmp.size(); ++k)
				{
					face.Center += tmp[k];
					face.Indices.push_back(mVertices.findLinearSearch(tmp[k]));
				}
				face.Center /= tmp.size();

				tmp.clear();

				Plane *tmpPlane = new Plane();
				tmpPlane->normal = face.Plane.normal.absolute();
				tmpPlane->dist = face.Plane.dist;

				if(mDefaultPlanes.findLinearSearch(*tmpPlane) == mDefaultPlanes.size())
				{
					mDefaultPlanes.push_back(*tmpPlane);
				}
				else
				{
					delete tmpPlane;
				}

				mHulls[i]->GetEdgesFromFace(j, tmp);
				for(int k = 0; k < tmp.size(); k+=2)
				{
					Edge* edge = new Edge(mVertices.findLinearSearch(tmp[k]), mVertices.findLinearSearch(tmp[k+1]));
					int index = mEdges.findLinearSearch(*edge);
					if(index < mEdges.size())
					{
						delete edge;						
					}
					else
					{
						mEdges.push_back(*edge);
					}
					face.Edges.push_back(index);

				}

				mFaces.push_back(face);

			}
		}



		std::cout << "Weryfikacja:" << std::endl;
		for(int i = 0; i < mFaces.size(); ++i)
		{
			std::cout << "Sciana " << i << std::endl;
			std::cout << "\tNormal: ";
			//Triangle::Print(mFaces[i].Plane.normal);
			std::cout << "\tDist: " << mFaces[i].Plane.dist << std::endl;
			std::cout << "\tWierzcholki: " << std::endl;
			for(int j = 0; j < mFaces[i].Indices.size(); ++j)
			{
				std::cout << "\t\t";
				//Triangle::Print(mVertices[mFaces[i].Indices[j]]);
				std::cout << std::endl;
			}
			std::cout << "\tKrawedzie:" << std::endl;
			for(int j = 0; j < mFaces[i].Edges.size(); ++j)
			{
				std::cout << "\t\t";
				//Triangle::Print(mVertices[mEdges[mFaces[i].Edges[j]].Begin]);
				std::cout << "\t";
				//Triangle::Print(mVertices[mEdges[mFaces[i].Edges[j]].End]);
				std::cout << std::endl;
			}
			std::cout << std::endl;
		}

	}

	/// Destruktor.
	~Decomposition(void)
	{
		/*if(this->mTestCube)
		{
			this->mWorld->removeCollisionObject(mTestCube);
			delete mTestCube->getCollisionShape();
			delete mTestCube;
			mTestCube = nullptr;
		}*/
	}

	/** Wyznacza odleglosc punktu od plaszczyzy
	* @param v Wspolrzedne punktu.
	* @param p Informacje o plaszczyznie.
	* @param pointOnPlane Wspolrzedne punktu lezacego na plaszczyznie.
	*/
	float PoitToPlaneDist(btVector3& v, Plane& p, btVector3& pointOnPlane)
	{
		/*std::cout << "\tPoint: ";
		Triangle::Print(v);
		std::cout << "\tNormal: ";
		Triangle::Print(p.normal);
		std::cout << "\tDist: " << p.dist << std::endl;*/

		if(p.normal.dot(v) == p.dist)
			return 0;

		float d = -pointOnPlane.dot(p.normal);
		float t = -d - p.normal.x() * v.x() - p.normal.y() * v.y() - p.normal.z() * v.z();
		t /= p.normal.x() * p.normal.x() + p.normal.y() * p.normal.y() + p.normal.z() * p.normal.z();

		btVector3 tmp(v.x() + t * p.normal.x(), v.y() + t * p.normal.y(), v.z() + t * p.normal.z());

		/*std::cout << "\tt: " << t << "\t";
		Triangle::Print(v);
		std::cout << "\t";
		Triangle::Print(tmp);
		std::cout << std::endl;*/

		return tmp.distance(v);
	}

	/** Szuka najblizszej sciany.
	* @param index Numer sciany, dla ktorej jest szukana najblizsza sciana
	* @param maxIndex Maksynalny numer sciany
	* @param result Lista indeksow najblizszych scian
	* @param useNormal Flaga okreslajaca, czy obliczenia bazuja na normalnych scian.
	*/
	void FindNearestVisibleFace(int index, int maxIndex, btAlignedObjectArray<int>& result, bool useNormal = false)
	{
		
		//std::cout << "\tFinding clossest for " << index << " " << maxIndex << std::endl;
		float minDistance;
		int min;
		min = index;
		minDistance = FLT_MAX;

		
		int max = maxIndex > 0 ? maxIndex : mFaces.size();

		for(int i = START_INDEX; i < max; ++i)
		{
			if(i == index || !mFaces[i].CanBeUsed)
				continue;

			float planeAngle = mFaces[index].Plane.normal.dot(mFaces[i].Plane.normal);
			
			if(mFaces[index].Plane.normal == mFaces[i].Plane.normal && mFaces[index].Plane.dist == mFaces[i].Plane.dist)
				continue;

			if(this->IsFaceVisible(index, i, useNormal) && planeAngle != 0.0f)
			{
				std::cout << "\tDistance between face " << std::endl;
				for(int j = 0; j < mFaces[i].Indices.size(); ++j)
				{
				std::cout << "\t\t";
				//Triangle::Print(mVertices[mFaces[i].Indices[j]]);
				std::cout << std::endl;
				}

				float tmpDistance = this->PoitToPlaneDist(mFaces[index].Center, *static_cast<Plane*>(&mFaces[i].Plane), mVertices[mFaces[i].Indices[0]]);
				//std::cout << dist << std::endl;

				if(minDistance == tmpDistance)
				{
					std::cout << "\t\tInserting " << i << " face to clossest faces " << std::endl;
					result.push_back(i);
				}
				else if(tmpDistance < minDistance)
				{
					if(planeAngle == -1 && tmpDistance == 0.f)
					{
						continue;
					}
					std::cout << "\t\tClearing clossest faces. Added " << i << " face with " << tmpDistance << " dist." << std::endl;
					result.clear();
					minDistance = tmpDistance;
					result.push_back(i);
				}

			}
		}

	}

	/** Tworzy nowe sciany na podstawie stworzonych bryl.
	* @param hull Wskaznik na nowo wyznaczona bryle.
	* @param usedFaces Lista uzytych scian.
	*/
	void GetNewFacesFromHull(ConvexHull* hull, btAlignedObjectArray<int>& usedFaces)
	{
		for(int i = 0; i < hull->faces.size(); ++i)
		{
			btAlignedObjectArray<btVector3> points;
			hull->GetVerticesFromFace(i, points);
			Plane plane = *static_cast<Plane*>(&hull->planes[i]);
			std::cout << "\tAnalyzing face " << i << "\t";

			Plane tmpPlane;
			tmpPlane.normal = plane.normal.absolute();
			tmpPlane.dist = plane.dist;
			if(mDefaultPlanes.findLinearSearch(tmpPlane) < mDefaultPlanes.size())
			{
				std::cout << std::endl;	
				continue;
			}

			Face *f = new Face(2);
			f->Center.setZero();
			for(int j = 0; j < points.size(); ++j)
			{
				f->Indices.push_back(mVertices.findLinearSearch(points[j]));
				f->Center += mVertices[f->Indices[f->Indices.size() - 1]];
			}

			f->Center /= points.size();

			points.clear();

			f->Plane = plane;
			
			int index = mFaces.findLinearSearch(*f);
			if(index < mFaces.size())
			{
				std::cout << std::endl;	
				delete f;
			}
			else
			{
				hull->GetEdgesFromFace(i, points);
				std::cout << " NEW\t\t";
				//Triangle::Print(f->Center);
				std::cout << std::endl;
				for(int j = 0; j < points.size(); ++j)
				{
					f->Edges.push_back(mVertices.findLinearSearch(points[j]));					
				}

				for(int j = 0; j < f->Indices.size(); ++j)
				{
					std::cout << "\t\t\t";
					//Triangle::Print(mVertices[f->Indices[j]]);
					std::cout << std::endl;
				}

				points.clear();

				mFaces.push_back(*f);
				
				usedFaces.push_back(index);
			}
		}
	}

	/** Dodaje nowa bryle
	* @param hull Wskaznik na nowa bryle.
	* @param usedPoints Lista wykorzystanych wierzcholkow.
	* @param usedFaces Lista wykorzystanych scian.
	* @return true, jesli bryla zostala dodana.
	*/
	bool AddNewConvexHull(ConvexHull* hull, btAlignedObjectArray<int>& usedPoints, btAlignedObjectArray<int>& usedFaces)
	{
		btAlignedObjectArray<btVector3> vertices;
		for(int i = 0; i < usedPoints.size(); ++i)
		{
			if(vertices.findLinearSearch(mVertices[usedPoints[i]]) == vertices.size())
				vertices.push_back(mVertices[usedPoints[i]]);
		}

		std::cout << "\tUsedPoints: " << usedPoints.size() << std::endl;
		
		hull->compute(&vertices[0].getX(), sizeof(btVector3), vertices.size(),0,0);
		hull->CorrectPrecision();
		hull->ComputePlanes();

		vertices.clear();

		bool deleteHull = false;
		for(int j = 0; j < mHulls.size(); ++j)
		{
			if(hull->Intersect(*mHulls[j]))
			{
				deleteHull = true;
				break;
			}
		}

		if(deleteHull)
		{			
			return false;
		}

		

		return !deleteHull;
	}

	int Decomp2(btAlignedObjectArray<ConvexHull>& OutPolygons, btAlignedObjectArray<ConvexHull>& Obstacles)
	{
		std::cout << "\n=================================================================\n" << std::endl;
		for(int i = 0; i < mHulls.size(); ++i)
		{
			Obstacles.push_back(*mHulls[i]);
		}

		int inputFacesCount = mFaces.size();
		int inputEdgesCount = mEdges.size();
		
		btAlignedObjectArray<int> faceOrder;

#pragma region All
		for(int i = 0; i < mFaces.size(); ++i)
		{
			if(!mFaces[i].CanBeUsed)
				continue;
			std::cout << "Face " << i << std::endl;
			btAlignedObjectArray<int> usedVertices, usedFaces, nearestFaces;
			usedFaces.push_back(i);

			for(int j = 0; j < mFaces[i].Indices.size(); ++j)
			{
				std::cout << "\t";
				//Triangle::Print(mVertices[mFaces[i].Indices[j]]);
				std::cout << std::endl;
				usedVertices.push_back(mFaces[i].Indices[j]);
			}



#pragma region Nearest face
			this->FindNearestVisibleFace(i,  i < START_INDEX ? inputFacesCount : mFaces.size(), nearestFaces, i > START_INDEX);
			if(nearestFaces.size() > 0)
			{
				for(int j = 0; j < nearestFaces.size(); ++j)
				{
					std::cout << "\t\tInserting face " << nearestFaces[j] << std::endl;
					if(usedFaces.findLinearSearch(nearestFaces[j]) == usedFaces.size())
					{
						usedFaces.push_back(nearestFaces[j]);
					}

					for(int k = 0; k < mFaces[nearestFaces[j]].Indices.size(); ++k)
					{
						int index = mFaces[nearestFaces[j]].Indices[k];
						if(usedVertices.findLinearSearch(index) == usedVertices.size())
						{
							usedVertices.push_back(index);
						}
					} // end k
				} // end j nearest face
			} // end if nearest face
#pragma endregion
#pragma region Searching rest points
			for(int j = START_INDEX; j < inputFacesCount; ++j)
			{
				if(!mFaces[j].CanBeUsed || usedFaces.findLinearSearch(j) == usedFaces.size())
					continue;
				
				if(this->IsFaceVisible(i, j))
				{
					std::cout << "\t\tInserting face " << j << std::endl;
					for(int k = 0; k < mFaces[j].Indices.size(); ++k)
					{
						int vertexIndex = mFaces[j].Indices[k];
						if(this->IsVisibleWithOthers(usedVertices, vertexIndex) && 
							usedVertices.findLinearSearch(vertexIndex) == usedVertices.size())
						{
							usedVertices.push_back(vertexIndex);
						}
					}
				}

			} // end j
#pragma endregion

			if(usedFaces.size() < 2)
				continue;

			ConvexHull* hull = new ConvexHull();
			if(this->AddNewConvexHull(hull, usedVertices, usedFaces))
			{

				if(OutPolygons.findLinearSearch(*hull) == OutPolygons.size())
				{
					std::cout << "\tHull: " << std::endl;
					for(int j = 0; j < hull->vertices.size(); ++j)
					{
						std::cout << "\t\t";
						//Triangle::Print(hull->vertices[j]);
						std::cout << std::endl;
					}

					

					//this->GetNewFacesFromHull(hull, usedFaces);
					OutPolygons.push_back(*hull);
					mHulls.push_back(hull);

					for(int j = 0; j < usedFaces.size(); ++j)					
					{
						std::cout << "\t\t\tUsing face " << usedFaces[j] << " ( " << mFaces[usedFaces[j]].count << " " << (mFaces[usedFaces[j]].CanBeUsed ? "true" : "false") << " - ";
						mFaces[usedFaces[j]].Use();
						std::cout << mFaces[usedFaces[j]].count << " " << mFaces[usedFaces[j]].CanBeUsed << " ) " << std::endl;
					}

					std::cout << "!!!!!!!! ADDED !!!!!!!!!" << std::endl << std::endl;
				}
				else
				{
					delete hull;
					hull = nullptr;
				}
			}
			else
			{
				delete hull;
				hull = nullptr;
			}
		}

#pragma endregion
		/*
		std::cout << "Niewykorzystane sciany: " << std::endl;
		
		for(int i = START_INDEX; i < mFaces.size(); ++i)
		{
			if(!mFaces[i].CanBeUsed)
				continue;

			std::cout << "\tFace " << i;
			btAlignedObjectArray<int> usedVertices, usedFaces, nearestFaces;

			btAlignedObjectArray<btVector3> tmp;

			for(int j = 0; j < mFaces[i].Indices.size(); ++j)
			{
				int index = mFaces[i].Indices[j];

				usedVertices.push_back(index);
				std::cout << "\t\t";
				Triangle::Print(mVertices[index]);
				std::cout << std::endl;			

				tmp.push_back(mVertices[index]);

			} // end j

			ConvexHull c;
			c.compute(&tmp[0].getX(), sizeof(btVector3), tmp.size(), 0,0);
			c.CorrectPrecision();
			c.ComputePlanes();
			OutPolygons.push_back(c);
			tmp.clear();
			/*
#pragma region Nearest
			this->FindNearestVisibleFace(i, 0, nearestFaces);
			if(nearestFaces.size() > 0)
			{
				for(int j = 0; j < nearestFaces.size(); ++j)
				{

					if(usedFaces.findLinearSearch(nearestFaces[j]) == usedFaces.size())
					{
						usedFaces.push_back(nearestFaces[j]);
					}

					for(int k = 0; k < mFaces[nearestFaces[j]].Indices.size(); ++k)
					{
						int index = mFaces[nearestFaces[j]].Indices[k];
						if(usedVertices.findLinearSearch(index) == usedVertices.size())
						{
							usedVertices.push_back(index);
						}
					} // end k
				} // end j nearest face
			} // end if nearest face
			
#pragma endregion 


			ConvexHull* hull = new ConvexHull();
			if(this->AddNewConvexHull(hull, usedVertices, usedFaces))
			{

				if(OutPolygons.findLinearSearch(*hull) == OutPolygons.size())
				{
					std::cout << "\tHull: " << std::endl;
					for(int j = 0; j < hull->vertices.size(); ++j)
					{
						std::cout << "\t\t";
						Triangle::Print(hull->vertices[j]);
						std::cout << std::endl;
					}

					std::cout << "!!!!!!!! ADDED !!!!!!!!!" << std::endl;

					this->GetNewFacesFromHull(hull, usedFaces);
					OutPolygons.push_back(*hull);

					for(int j = 0; j < usedFaces.size(); ++j)					
					{
						mFaces[usedFaces[j]].Use();
					}
				}
				else
				{
					delete hull;
					hull = nullptr;
				}
			}
			else
			{
				delete hull;
				hull = nullptr;
			}
			
			std::cout << std::endl;
		}*/

		return 0;
}

	int Decomp(btAlignedObjectArray<ConvexHull>& OutPolygons, btAlignedObjectArray<ConvexHull>& Obstacles)
	{
		int n = mVertices.size();
		int m = mFaces.size();
		std::cout << "Dekompozycja: " << n << "\t" << m <<  std::endl;
		for(int k = 0; k < mHulls.size(); ++k)
		{
			Obstacles.push_back(*mHulls[k]);
		}

		btAlignedObjectArray<int> soloFaces;

		for(int i = 0; i < m; ++i)
		{
			if(!mFaces[i].CanBeUsed)
				continue;

			btAlignedObjectArray<int> tmpList, usedFaces, notVisibleFaces;
			usedFaces.push_back(i);
			tmpList.copyFromArray(mFaces[i].Indices);
			std::cout << "Processing face " << i << ":" << std::endl;
			for(int j = 0; j < tmpList.size(); ++j)
			{
				std::cout << "\t";
				//Triangle::Print(mVertices[tmpList[j]]);
				std::cout << std::endl;
			} // end j

			btAlignedObjectArray<int> clossestFace;
			this->FindNearestVisibleFace(i, i < START_INDEX ? m : mFaces.size(), clossestFace);
			if(clossestFace.size())
			{
				std::cout << "\n\tNearest faces: " << clossestFace.size() << std::endl;

				for(int j = 0; j < clossestFace.size(); ++j)
				{
					std::cout << "\t\tFace " << j << std::endl;
					for(int k = 0; k < mFaces[clossestFace[j]].Indices.size(); ++k)
					{
						std::cout << "\t\t\t";
						//Triangle::Print(mVertices[mFaces[clossestFace[j]].Indices[k]]);
						std::cout << std::endl;
						if(tmpList.findLinearSearch(mFaces[clossestFace[j]].Indices[k]) == tmpList.size())
						{
							tmpList.push_back(mFaces[clossestFace[j]].Indices[k]);
						}
					} // end k

					usedFaces.push_back(clossestFace[j]);
				} // end j
			} // end if
			std::cout << std::endl;
			for(int j = START_INDEX; j < mFaces.size(); ++j)
			{
				if(usedFaces.findLinearSearch(j) < usedFaces.size())
					continue;
				if(!this->IsFaceVisible(i,j))
					continue;

				bool allCanBeAdded = true;
				for(int k = 0; k < mFaces[j].Indices.size(); ++k)
				{
					if(!this->IsVisibleWithOthers(tmpList, mFaces[j].Indices[k]))
					{
						allCanBeAdded = false;
						break;
					}
				} // end k

				if(allCanBeAdded)
				{
					usedFaces.push_back(j);

					for(int k = 0; k < mFaces[j].Indices.size(); ++k)
					{						
						if(tmpList.findLinearSearch(mFaces[j].Indices[k]) == tmpList.size())
						{
							std::cout << "\tAdding ";
							//Triangle::Print(mVertices[mFaces[j].Indices[k]]);
							std::cout << std::endl;
							tmpList.push_back(mFaces[j].Indices[k]);
						}
					} // end k
				} // end if
				else
				{
					notVisibleFaces.push_back(j);
				}
			} // end j

			for(int j = 0; j < notVisibleFaces.size(); ++j)
			{
				for(int k = 0; k < mFaces[notVisibleFaces[j]].Edges.size(); ++k)
				{
					int v1 = mEdges[mFaces[notVisibleFaces[j]].Edges[k]].Begin;
					int v2 = mEdges[mFaces[notVisibleFaces[j]].Edges[k]].End;

					if(this->IsVisibleWithOthers(tmpList, v1) && this->IsVisibleWithOthers(tmpList, v2))
					{
						std::cout << "\t\tAdding ";
						//Triangle::Print(mVertices[v1]);
						std::cout << "\t\tAdding ";
						//Triangle::Print(mVertices[v2]);
						std::cout << std::endl;

						if(tmpList.findLinearSearch(v1) == tmpList.size())
							tmpList.push_back(v1);

						if(tmpList.findLinearSearch(v2) == tmpList.size())
							tmpList.push_back(v2);
					}

				} // end k
			} // end j

			btAlignedObjectArray<btVector3> vertices;
			std::cout << std::endl;
			for(int j = 0; j < tmpList.size(); ++j)
			{
				std::cout << "\t\t" << tmpList[j]<< ": ";
				//Triangle::Print(mVertices[tmpList[j]]);
				std::cout << std::endl;
				vertices.push_back(mVertices[tmpList[j]]);
			} // end j

			for(int j = 0; j < mEdges.size(); ++j)
			{
			} // end j
		

			std::cout << "\n\tCreating hull (" << tmpList.size() << ", " << vertices.size() << ", ";
			tmpList.clear();
			ConvexHull* c = new ConvexHull();
			c->compute(&vertices[0].getX(), sizeof(btVector3), vertices.size(),0,0);
			c->CorrectPrecision();
			c->ComputePlanes();

			std::cout << c->vertices.size() << ")\tused faces:" << usedFaces.size() << std::endl;

			if(this->AddNewConvexHull(c, tmpList, usedFaces))
			{
				std::cout << "\t\tUsed faces: " << usedFaces.size() << std::endl;
				if(OutPolygons.findLinearSearch(*c) == OutPolygons.size())
				{
					for(int j = 0; j < usedFaces.size(); ++j)
					{
						mFaces[usedFaces[j]].Use();
					}

					OutPolygons.push_back(*c);
					mHulls.push_back(c);
					std::cout << "!!!!!!!!!!!!ADDED!!!!!!!!!!!!" << std::endl;
				}
				else
					delete c;
			}
			else
			{
				delete c;
				c = 0;
			}
		} // end i


		return OutPolygons.size();
	}

	int PerformDecomposition(btAlignedObjectArray<ConvexHull>& OutPolygons, btAlignedObjectArray<ConvexHull>& Obstacles)
	{
		int n = mVertices.size();
		int m = mFaces.size();
		std::cout << "Dekompozycja: " << n << "\t" << m <<  std::endl;
		for(int k = 0; k < mHulls.size(); ++k)
		{
			Obstacles.push_back(*mHulls[k]);
		}
		///*std::cout << "Callback infos: " << std::endl;
		//std::cout << "\tClosestHitFraction: " << callback.m_closestHitFraction << std::endl;
		//std::cout << "\tObj: " << callback.m_collisionObjects.size() << std::endl;
		//std::cout << "\tHitPointWorld:" << callback.m_hitFractions.size() << std::endl;*/
		//
		//int index = -1;

		//while(++index < m - 6)
		//{
		//	btAlignedObjectArray<int> list;
		//	list.copyFromArray(mFaces[index].Indices);

		//	std::cout << "Trying for: " << std::endl;
		//	for(int i = 0 ; i < list.size(); ++i)
		//	{
		//		std::cout << "\t";
		//		Triangle::Print(mVertices[list[i]]);
		//		std::cout << std::endl;
		//	}
		//	std::cout << std::endl;

		//	int start = 0;
		//	int end = m - 6;

		//	for(int iter = 0; iter < 2; ++iter)
		//	{
		//		if(iter == 0)
		//		{
		//			start = 0;
		//			end = m - 6;
		//		}
		//		else
		//		{
		//			start = m - 6;
		//			end = m;
		//		}

		//		for(int i = start; i < end; ++i)
		//		{
		//			if(i == index)
		//				continue;

		//			bool cancel = false;
		//			std::cout << "\t\t";
		//			Triangle::Print(mFaces[index].Center);
		//			std::cout << "\t";
		//			Triangle::Print(mFaces[i].Center);
		//			for(int j = 0; j < mHulls.size(); ++j)
		//			{
		//				if(mHulls[j]->RayTest(mFaces[index].Center, mFaces[i].Center))
		//				{
		//					cancel = true;
		//					break;
		//				}
		//			}
		//			std::cout << "\t" << (cancel ? " hit" : " clear") << std::endl;
		//			if(cancel)
		//				continue;
		//			/*btCollisionWorld::AllHitsRayResultCallback callback(mFaces[index].Center, mFaces[i].Center);
		//			mWorld->rayTest(mFaces[index].Center, mFaces[i].Center, callback);
		//			std::cout << "\t\t";
		//			Triangle::Print(mFaces[index].Center);
		//			std::cout << "\t";
		//			Triangle::Print(mFaces[i].Center);
		//			std::cout << (callback.hasHit() ? "" : " not") << " colide." << std::endl;
		//			if(callback.hasHit())
		//				continue;*/

		//			bool allVisible = true;
		//			for(int j = 0; j < mFaces[i].Indices.size(); ++j)
		//			{
		//				if(list.findLinearSearch(mFaces[i].Indices[j]))
		//					continue;
		//				if(!this->IsVisibleWithOthers(list, mFaces[i].Indices[j]))
		//				{
		//					allVisible = false;
		//					break;
		//				}
		//			}

		//			if(allVisible)
		//			{
		//				for(int j = 0; j < mFaces[i].Indices.size(); ++j)
		//				{
		//					if(list.findLinearSearch(mFaces[i].Indices[j]) == list.size())
		//						list.push_back(mFaces[i].Indices[j]);
		//				}
		//			}

		//		}
		//	}

		//	ConvexHull* c = new ConvexHull();
		//	btAlignedObjectArray<btVector3> tmp;
		//	std::cout << "\tPolygon (vectors):" << std::endl;
		//	for(int i = 0; i < list.size(); ++i)
		//	{
		//		tmp.push_back(mVertices[list[i]]);
		//		std::cout << "\t\t";
		//		Triangle::Print(mVertices[list[i]]);
		//		std::cout << std::endl;
		//	}

		//	c->compute(&tmp[0].getX(), sizeof(btVector3), tmp.size(),0,0);
		//	c->CorrectPrecision();
		//	c->ComputePlanes();

		//	bool add = true;

		//	if(OutPolygons.findLinearSearch(*c) == OutPolygons.size())
		//	{
		//		for(int i = 0; i < mHulls.size(); ++i)
		//		{
		//			if(mHulls[i]->Intersect(*c))
		//			{
		//				add = false;
		//				break;
		//			}
		//		}
		//	}
		//	else
		//	{
		//		add = false;
		//	}

		//	std::cout << "\t\tPolygon:" << std::endl;
		//	for(int i = 0; i < c->vertices.size(); ++i)
		//	{
		//		std::cout << "\t\t\t";
		//		Triangle::Print(c->vertices[i]);
		//		std::cout << std::endl;
		//	}

		//	if(add)
		//	{
		//		std::cout << "!!!!!!!!!!!ADDED!!!!!!!!!!!!!" << std::endl;
		//		OutPolygons.push_back(*c);
		//	}
		//	else
		//	{
		//		delete c;
		//		c = 0;
		//	}			
		//}

		//return 0;

		for(int i = 0; i < mFaces.size(); ++i)
		{
			if(!mFaces[i].CanBeUsed)
				continue;

			btAlignedObjectArray<int> possibleVisibleFaces, possibleCanUseEdges, tmpList, usedEdges, usedFaces;
			tmpList.copyFromArray(mFaces[i].Indices);

			std::cout << "Performing for: " << std::endl;
			for(int k = 0; k < tmpList.size(); ++k)
			{
				std::cout << "\t";
				//Triangle::Print(mVertices[tmpList[k]]);
				std::cout << std:: endl;
			}

#pragma region Jazda po scianach
			for(int j = 0; j < mFaces.size(); ++j)
			{


				if(i == j || !mFaces[j].CanBeUsed)
				{
					//std::cout << "PASS" << std::endl;
					continue;
				}



				btCollisionWorld::ClosestRayResultCallback rayTestCallback(mFaces[i].Center, mFaces[j].Center);
				mWorld->rayTest(mFaces[i].Center, mFaces[j].Center, rayTestCallback);
				if(rayTestCallback.hasHit())
				{
					/*std::cout << "NOT VISIBLE\t";
					Triangle::Print(rayTestCallback.m_hitPointWorld);
					std::cout << std::endl;*/
					continue;
				}

				std::cout << "\n\tAganist face:" << std::endl;

				for(int k = 0; k < mFaces[j].Indices.size(); ++k)
				{
					std::cout << "\t";
					//Triangle::Print(mVertices[mFaces[j].Indices[k]]);
					std::cout << std::endl;
				}

				/*if(mFaces[i].Plane.normal == mFaces[j].Plane.normal && mFaces[i].Plane.dist == mFaces[j].Plane.dist)
				{
				std::cout << "MOVED TO POSSIBLE VISIBLE" << std::endl;
				possibleVisibleFaces.push_back(j);
				continue;
				}*/

				bool allInFrontOf = true;

				for(int k = 0; k < mFaces[j].Indices.size(); ++k)
				{
					if(mFaces[i].Plane.normal.dot(mVertices[mFaces[j].Indices[k]]) < mFaces[i].Plane.dist)
					{
						allInFrontOf = false;
						break;
					}
				}
				std::cout << "\t(All in front of: " << (allInFrontOf ? "y" : "n") << ")" << std::endl;
				if(!allInFrontOf)
				{

					for(int k = 0; k < mFaces[j].Edges.size(); ++k)
					{
						if(usedEdges.findLinearSearch(mFaces[j].Edges[k]) < usedEdges.size())
							continue;

						int v1 = mEdges[mFaces[j].Edges[k]].Begin;
						int v2 = mEdges[mFaces[j].Edges[k]].End;

						if(mFaces[i].Plane.normal.dot(mVertices[v1]) >= mFaces[i].Plane.dist && mFaces[i].Plane.normal.dot(mVertices[v2]) >= mFaces[i].Plane.dist)
						{
							if(this->IsVisibleWithOthers(tmpList, v1) && this->IsVisibleWithOthers(tmpList, v2))
							{
								if(tmpList.findLinearSearch(v1) == tmpList.size())
									tmpList.push_back(v1);
								if(tmpList.findLinearSearch(v2) == tmpList.size())
									tmpList.push_back(v2);

								std::cout << "\n\t\tAdded: ";
								//Triangle::Print(mVertices[v1]);
								std::cout << " and ";
								//Triangle::Print(mVertices[v2]);
								std::cout << std::endl;
								usedEdges.push_back(mFaces[j].Edges[k]);
							}
						}
					}
				}
				else
				{
					bool allVisible = true;
					for(int k = 0; k < mFaces[j].Indices.size(); ++k)
					{
						if(!this->IsVisibleWithOthers(tmpList, mFaces[j].Indices[k]))
						{
							allVisible = false;
							break;
						}
					}
					std::cout << "\t(All visible: " << (allVisible ? "y" : "n") << ")" << std::endl;
					if(!allVisible)
						continue;
					for(int k = 0; k < mFaces[j].Indices.size(); ++k)
					{
						if(tmpList.findLinearSearch(mFaces[j].Indices[k]) == tmpList.size())
						{
							std::cout << "\n\t\tAdding ";
							//Triangle::Print(mVertices[mFaces[j].Indices[k]]);
							//std::cout << std::endl;
							tmpList.push_back(mFaces[j].Indices[k]);
						}
					}

					usedFaces.push_back(j);
				}

			}
#pragma endregion

#pragma region Possible can use edge

			if(possibleCanUseEdges.size())
			{
				std::cout << "\nPOSSIBLE CAN USE EDGES" << std::endl;
				for(int j = 0; j < possibleCanUseEdges.size(); ++j)
				{
					for(int k = 0; k < mFaces[possibleCanUseEdges[j]].Edges.size(); ++j)
					{
						if(usedEdges.findLinearSearch(mFaces[possibleCanUseEdges[j]].Edges[k]) == usedEdges.size())
						{
							int v1 = mEdges[mFaces[possibleCanUseEdges[j]].Edges[k]].Begin;
							int v2 = mEdges[mFaces[possibleCanUseEdges[j]].Edges[k]].End;

							if(mFaces[i].Plane.normal.dot(mVertices[v1]) >= mFaces[i].Plane.dist && mFaces[i].Plane.normal.dot(mVertices[v2]) >= mFaces[i].Plane.dist)
							{
								if(this->IsVisibleWithOthers(tmpList, v1) && this->IsVisibleWithOthers(tmpList, v2))
								{
									if(tmpList.findLinearSearch(v1) == tmpList.size())
										tmpList.push_back(v1);
									if(tmpList.findLinearSearch(v2) == tmpList.size())
										tmpList.push_back(v2);

									std::cout << "\n\t\tAdded: ";
									//Triangle::Print(mVertices[v1]);
									std::cout << " and ";
									//Triangle::Print(mVertices[v2]);
									std::cout << std::endl;
									usedEdges.push_back(mFaces[possibleCanUseEdges[j]].Edges[k]);
								}
							}
						}
					}
				}
			}

#pragma endregion

#pragma region Jazda po prawdopodobnie widocznych

			if(possibleVisibleFaces.size() > 0)
			{
				std::cout << "\nPROBABLY VISIBLE" << std::endl;
				std::cout << "\tAlready added:" << std::endl;

				for(int j = 0; j < tmpList.size(); ++j)
				{
					std::cout << "\t\t";
					//Triangle::Print(mVertices[tmpList[j]]);
					std::cout << std::endl;
				}


				for(int j = 0; j < possibleVisibleFaces.size(); ++j)
				{
					if(!mFaces[possibleVisibleFaces[j]].CanBeUsed)
						continue;
					btCollisionWorld::ClosestRayResultCallback callback(mFaces[i].Center, mFaces[possibleVisibleFaces[j]].Center);
					mWorld->rayTest(mFaces[i].Center, mFaces[possibleVisibleFaces[j]].Center, callback);
					if(callback.hasHit())
						continue;

					std::cout << "\tAganist face" << std::endl;
					bool alreadyIn = true;
					for(int k = 0; k < mFaces[possibleVisibleFaces[j]].Indices.size(); ++k)
					{
						std::cout << "\t\t";
						//Triangle::Print(mVertices[mFaces[possibleVisibleFaces[j]].Indices[k]]);
						std::cout << std::endl;

						if(alreadyIn && tmpList.findLinearSearch(mFaces[possibleVisibleFaces[j]].Indices[k]) == tmpList.size())
							alreadyIn = false;
					}

					if(alreadyIn)
					{
						std::cout << "\tAlready used." << std::endl;
						usedFaces.push_back(possibleVisibleFaces[j]);
						continue;
					}

					bool allVisible;
					for(int k = 0; k < mFaces[possibleVisibleFaces[j]].Indices.size(); ++k)
					{
						if(!this->IsVisibleWithOthers(tmpList, mFaces[possibleVisibleFaces[j]].Indices[k]))
						{
							allVisible = false;
							break;
						}
					}

					std::cout << "(All visible: " << (allVisible ? "y" : "n") << ")" << std::endl;

					if(allVisible)
					{
						for(int k = 0; k < mFaces[possibleVisibleFaces[j]].Indices.size(); ++k)
						{
							if(tmpList.findLinearSearch(mFaces[possibleVisibleFaces[j]].Indices[k]) == tmpList.size())
								tmpList.push_back(mFaces[possibleVisibleFaces[j]].Indices[k]);
						}
						usedFaces.push_back(possibleVisibleFaces[j]);
					}
				}
			}

#pragma endregion


			btAlignedObjectArray<btVector3> vectors;
			for(int j = 0; j < tmpList.size(); ++j)
				vectors.push_back(mVertices[tmpList[j]]);

			ConvexHull *c = new ConvexHull();
			c->compute(&vectors[0].getX(), sizeof(btVector3), vectors.size(),0,0);
			c->CorrectPrecision();
			c->ComputePlanes();

			bool correct = true;

			if(OutPolygons.findLinearSearch(*c) == OutPolygons.size())
			{
				for(int j = 0; j < mHulls.size(); ++j)
				{
					if(mHulls[j]->Intersect(*c))
					{
						correct = false;
						break;
					}
				}
			}
			else
				correct = false;

			std::cout << "\tPolygon:" << std::endl;
			for(int k = 0; k < tmpList.size(); ++k)
			{
				std::cout << "\t\t";
				//Triangle::Print(mVertices[tmpList[k]]);
				std::cout << std::endl;
			}
			if(correct)
			{
				usedFaces.push_back(i);
				for(int k = 0; k < usedFaces.size(); ++k)
				{
					mFaces[usedFaces[k]].CanBeUsed = false;
					for(int l = 0; l < mFaces[usedFaces[k]].Edges.size();++l)
					{
						mEdges[mFaces[usedFaces[k]].Edges[l]].Counts++;
					}
				}
				OutPolygons.push_back(*c);


				std::cout << "\n!!!!!!!!!!ADDED!!!!!!!!!!!!!!!!" << std::endl;
			}
			else
			{
				delete c;
				c = 0;
			}

			std::cout << "=========================================" << std::endl << std::endl;
		}

		std::cout << "Statystki: " << std::endl;
		for(int i = 0; i < mEdges.size(); ++i)
		{
			std::cout << "\t";
			//Triangle::Print(mVertices[mEdges[i].Begin]);
			std::cout << " ";
			//Triangle::Print(mVertices[mEdges[i].End]);
			std::cout << " - " << mEdges[i].Counts << std::endl;

		}
		goto print_results;

		int counter = 0;
		for(int i = 0; i < n; ++i)
		{

			for(int j = 0; j < n; ++j)
			{
				std::cout << "\t" << mGraph[i][j];				
			}

			std::cout << std::endl;
		}

		std::cout << std::endl;


		for(int i = 0; i < mHulls.size(); ++i)
		{
			for(int j = 0; j < mHulls[i]->planes.size(); ++j)
			{
				Plane* tmpPlane = static_cast<Plane*>(&(mHulls[i]->planes[j]));

				btAlignedObjectArray<int> tmpList;
				btAlignedObjectArray<btVector3> outList;
				mHulls[i]->GetVerticesFromFace(j, outList);
				std::cout << "\n========================" << std::endl;
				std::cout << "Performing for\t";

				for(int k = 0; k < outList.size(); ++k)
				{
					int index = mVertices.findLinearSearch(outList[k]);
					if(index < mVertices.size())
					{
						//Triangle::Print(mVertices[index]);
						std::cout << "\n\t\t\t\t";
						tmpList.push_back(index);
					}
				}

				outList.clear();
				if(tmpList.size() == 0)
					continue;

				for(int k = 0; k < mVertices.size(); ++k)
				{
					btAlignedObjectArray<btVector3> planeTestVer;
					planeTestVer.push_back(mVertices[k]);
					if(btGeometryUtil::areVerticesBehindPlane(tmpPlane->normal, planeTestVer, 0.01f))
						continue;
					if(this->IsVisibleWithOthers(tmpList, k))
					{
						std::cout << "=> added" << std::endl;
						tmpList.push_back(k);
					}
				}

				for(int k = 0; k < tmpList.size(); ++k)
					outList.push_back(mVertices[tmpList[k]]);

				ConvexHull *c = new ConvexHull();
				c->compute(&outList[0].getX(), sizeof(btVector3), outList.size(),0,0);
				c->CorrectPrecision();
				if(OutPolygons.findLinearSearch(*c) == OutPolygons.size())
				{
					std::cout << "\nADDED" << std::endl;
					OutPolygons.push_back(*c);
				}
				else
				{
					delete c;
					c = 0;
				}

			}
		}

		/*std::cout << "\nJazda po AABB: " << mWorldHull->faces.size() << std::endl;

		for(int i = 0; i < mWorldHull->faces.size(); ++i)
		{
		Plane* tmpPlane = static_cast<Plane*>(&(mWorldHull->planes[i]));

		btAlignedObjectArray<int> tmpList;
		btAlignedObjectArray<btVector3> outList;
		mWorldHull->GetVerticesFromFace(i, outList);
		for(int k = 0; k < outList.size(); ++k)
		{
		int index = mVertices.findLinearSearch(outList[k]);
		if(index < mVertices.size())
		tmpList.push_back(index);
		}

		outList.clear();
		if(tmpList.size() == 0)
		continue;

		for(int k = 0; k < mVertices.size(); ++k)
		{
		if(this->IsVisibleWithOthers(tmpList, k))
		tmpList.push_back(k);
		}

		for(int k = 0; k < tmpList.size(); ++k)
		outList.push_back(mVertices[tmpList[k]]);

		ConvexHull *c = new ConvexHull();
		c->compute(&outList[0].getX(), sizeof(btVector3), outList.size(),0,0);
		c->CorrectPrecision();
		if(OutPolygons.findLinearSearch(*c) == OutPolygons.size())
		{
		OutPolygons.push_back(*c);
		}
		else
		{
		delete c;
		c = 0;
		}
		}*/
print_results:
		std::cout << "\n\tIl. polygonow: " << OutPolygons.size() << std::endl;
		for(int k = 0; k < OutPolygons.size(); ++k)
		{

			std::cout << k << ".\t" << OutPolygons[k].vertices.size() << std::endl;
			for(int l = 0; l < OutPolygons[k].vertices.size(); ++l)
			{
				if(OutPolygons[k].vertices.size() > 0)
				{
					std::cout << "\t";
					//Triangle::Print(OutPolygons[k].vertices[l]);
					std::cout << std::endl;
				}
			}
			std::cout << std::endl;
		}		

		return 0;
	}

private:

	const static int START_INDEX = 6;
	/// Lista wszystkich scian sceny.
	btAlignedObjectArray<Face> mFaces;
	/// Graf polaczen miedzy wierzcholkami
	boost::multi_array<float, 2> mGraph;
	/// Lista wierzcholkow sceny.
	btAlignedObjectArray<btVector3> mVertices;

	btVector3 mMinAABB, mMaxAABB, mStepSize;
	/// Wskaznik na klase swiata.
	btCollisionWorld* mWorld;

	/** Sprawdza, czy wszystkie wierzcholki dwoch scian sa widoczne
	* @param soruceFace Numer sciany "odniesienia".
	* @param targetFace Numer sciany do sprawdzenia.
	* @param useNormal Okresla, czy jest brana pod uwage normalna sciany.
	* @return true - sciany sa wzajemnie widoczne.
	*/
	bool AllVerticesVisible(int sourceFace, int targetFace, bool useNormal = false)
	{
		//std::cout << "\t\tAllVerticesVisible: " << sourceFace << "\t" << targetFace << "\tuseNormal " << useNormal << std::endl;
		if(sourceFace == targetFace)
			return true;


		Face f1 = mFaces[sourceFace];
		Face f2 = mFaces[targetFace];

		btCollisionWorld::AllHitsRayResultCallback callback(f1.Center, f2.Center);
		mWorld->rayTest(f1.Center, f2.Center, callback);
		if(callback.hasHit())
			return false;

		for(int i = 0; i < mHulls.size(); ++i)
		{
			if(mHulls[i]->RayTest(f1.Center, f2.Center, true))
				return false;
		}

		bool allVisible = true;
		if(useNormal)
		{
			for(int i = 0; i < f2.Indices.size(); ++i)
			{
				if(f1.Plane.normal.dot(mVertices[f2.Indices[i]]) < f1.Plane.dist)
				{
					allVisible = false;
					break;
				}
			}
		}
		if(!allVisible)
			return false;

		for(int i = 0; i < f1.Indices.size(); ++i)
		{
			for(int j = i + 1; j < f2.Indices.size(); ++j)
			{
				if(f1.Indices[i] != f2.Indices[j] && mGraph[f1.Indices[i]][f2.Indices[j]] <= 0)
				{
					allVisible = false;
					break;
				}
			}
		}

		std::cout << "\tAllVisible: " << allVisible << std::endl;
		return allVisible;
	}

	/** Okresla, czy dwie sciany sa widoczne
	* @param source Numer sciany, dla ktorej jest przeprowadzany test
	* @param target Numer sciany, ktora zostanie sprawdzona
	* @param useNormal Okresla, czy do obliczen zostanie wykorzystana normalna sciany.
	* @return true - sciany sa widoczne.
	*/
	bool IsFaceVisible(int source, int target, bool useNormal = false)
	{
		std::cout << "\t\tIsFaceVisible: " << source << "\t" << target;
		if(source == target)
		{
			std::cout << std::endl;
			return false;		
		}

		btCollisionWorld::AllHitsRayResultCallback callback(mFaces[source].Center, mFaces[target].Center);
		mWorld->rayTest(mFaces[source].Center, mFaces[target].Center, callback);
		if(callback.hasHit())
		{
			std::cout << std::endl;
			return false;
		}
		//std::cout << "FaceVisible: hulls - " << mHulls.size() << std::endl;
		for(int i = 0; i < mHulls.size(); ++i)
		{
			if(mHulls[i]->IsInside(mFaces[source].Center, true) || mHulls[i]->IsInside(mFaces[target].Center, true))
			{
				std::cout << std::endl;
				return false;
			}
			btAlignedObjectArray<btVector3> hitPoints;
			if(mHulls[i]->RayTest(mFaces[source].Center, mFaces[target].Center, hitPoints, true))
			{
				if(hitPoints.size() > 1)
				{
					std::cout << "\t\tHull " << i << " is collide with ray ";
					//Triangle::Print(mVertices[source]);
					std::cout << " and ";
					//Triangle::Print(mVertices[target]);
					std::cout << std::endl;
					return false;
				}
			}
		}

		return this->AllVerticesVisible(source, target, useNormal);

	}

	/** Sprawdza, czy dana sciana jest widoczna z pozostalymi
	* @param list Lista scian, z ktorymi bedzie sprawdzana zadana sciana.
	* @param index Numer sciany do sprawdzenia.
	* @return true - dana sciana jest widoczna z pozostalymi.
	*/
	bool IsVisibleWithOthers(btAlignedObjectArray<int>& list, int index)
	{
		if(list.findLinearSearch(index) < list.size())
			return true;
		//std::cout << "\n\tTrying to add ";
		////Triangle::Print(mVertices[index]);
		btVector3 center(mVertices[index]);

		btAlignedObjectArray<btVector3> tmpList;
		tmpList.push_back(mVertices[index]);

		for(int i = 0; i < list.size(); ++i)
		{
			/*std::cout << "\t\t";
			Triangle::Print(mVertices[list[i]]);
			std::cout << "\t" << mGraph[list[i]][index] << std::endl;
			*/
			if(list[i] != index && mGraph[list[i]][index] == 0)
			{
				return false;
			}
			center += mVertices[list[i]];
			tmpList.push_back(mVertices[list[i]]);
		}

		ConvexHull* hull = new ConvexHull();
		hull->compute(&tmpList[0].getX(), sizeof(btVector3), tmpList.size(), 0,0);
		hull->CorrectPrecision();
		hull->ComputePlanes();
		//std::cout << "\tCenter1: ";
		//Triangle::Print(center);
		center /= list.size() + 1;
		//std::cout << "\t";
		//Triangle::Print(center);

		//std::cout << std::endl;
		for(int i = 0; i < list.size(); ++i)
		{

			btCollisionWorld::AllHitsRayResultCallback callback(center, mVertices[list[i]]);
			mWorld->rayTest(center, mVertices[list[i]], callback);

			if(callback.hasHit())
				return false;

			for(int j = 0; j < mHulls.size(); ++j)
			{
				if(mHulls[j]->IsInside(center, true))
				{
					//std::cout << "\tis inside. \t";
					return false;
				}



				//std::cout << "\n\t" <<j << " ray test: ";
				btAlignedObjectArray<btVector3> hits;
				mHulls[j]->RayTest(center, mVertices[list[i]], hits, true);

				//for(int k = 0; k < hits.size(); ++k)
				//{
				//	std::cout << "\n\t\t\tHit point: ";
				//	Triangle::Print(hits[k]);
				//}



				if(hits.size() > 0 && !mHulls[j]->OnEdge(hits[0]))
				{
					//std::cout << "true";
					return false;
				}

				if(hull->Intersect(*mHulls[j]))
				{
					//std::cout << "\tintersect " << j << "\t";
					delete hull;
					return false;
				}
				/*std::cout << "false";*/
				//std::cout << std::endl;
			}			
		}
		delete hull;
		//std::cout << "\tkoniec.\t";
		return true;
	}

};

